#include"control_cmd/control_cmd_fsm.h"
//设置好对应的掩码，从右往左依次对应PX/PY/PZ/VX/VY/VZ/AX/AY/AZ/FORCE/YAW/YAW-RATE
#define VELTYPEMASK 0b011111000111 
#define POSTYPEMASK 0b101111111000
#define VELYAWTYPEMASK 0b101111000111

using namespace std;

namespace obstacle_avoid
{
    void CONTROL_CMD_FSM::init(ros::NodeHandle &nh)
    {
        // ObstacleAvoidFSM oa_fsm;
        // //get current cmd state
        // cmd_state = oa_fsm.getCurrentState();
        cmd_state = 0; //0 for INIT
        have_manual_cmd = false;
        have_odom = false;
        have_traj_cmd = false;
        start_return.data = false;
        is_backing_up = false;
        is_armed_prev = false;
        last_motion_direction = Eigen::Vector3d::Zero();
        
        nh.param("cmd/max_horizonal_vel_",max_horizonal_vel,2.0);
        nh.param("cmd/max_vertical_vel_",max_vertical_vel,2.0);
        nh.param("cmd/max_yaw_rate_",max_yaw_rate,60.0);
        nh.param("cmd/vel_limit_factor_",vel_limit_factor,0.5);
        nh.param("cmd/emergency_hover_duration_",emergency_hover_duration,3.0);

        //ros sub
        odom_sub = nh.subscribe("odom_topic",1,&CONTROL_CMD_FSM::odomCallback,this);
        manual_cmd_sub = nh.subscribe("manual_cmd_topic",1,&CONTROL_CMD_FSM::manualCmdCallback,this);
        state_sub = nh.subscribe("uav_state_topic",1,&CONTROL_CMD_FSM::stateCallback,this);
        obs_state_sub = nh.subscribe("obstacle_avoid_state_topic",1,&CONTROL_CMD_FSM::obsStateCallback,this);
        start_return_sub = nh.subscribe("return_home",1,&CONTROL_CMD_FSM::returnHomeCallback,this);
        traj_cmd_sub = nh.subscribe("traj_cmd",1,&CONTROL_CMD_FSM::trajCmdCallback,this);
        //ros pub
        cmd_pub = nh.advertise<mavros_msgs::PositionTarget>("cmd_topic",10);
        home_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("home_position",10);

        //ros timer
        control_cmd_timer = nh.createTimer(ros::Duration(0.02),&CONTROL_CMD_FSM::cmdTimerCallback,this);
    }

    void CONTROL_CMD_FSM::recordHomePosition()
    {
        if(!have_odom)
        {
            ROS_WARN_STREAM_THROTTLE(1.0,"Position data not available yet!");
            return;
        }

        home_pos.pose.position.x = odom_pos(0);
        home_pos.pose.position.y = odom_pos(1);
        home_pos.pose.position.z = 1.0;
        ROS_INFO("Home position recorded: (%f, %f, %f)", home_pos.pose.position.x, home_pos.pose.position.y, home_pos.pose.position.z);
    }

    void CONTROL_CMD_FSM::resetCurrentCmd(int cmd_state)
    {
        current_cmd.header.stamp = ros::Time::now();
        current_cmd.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
        switch (cmd_state)
        {
        case 1:
        {
            current_cmd.type_mask = VELTYPEMASK;
            current_cmd.velocity.x = 0.0;
            current_cmd.velocity.y = 0.0;
            current_cmd.velocity.z = 0.0;
            current_cmd.yaw_rate = 0.0;
            break;
        }
        case 2:
        {
            current_cmd.type_mask = VELTYPEMASK;
            current_cmd.velocity.x = 0.0;
            current_cmd.velocity.y = 0.0;
            current_cmd.velocity.z = 0.0;
            current_cmd.yaw_rate = 0.0;
            break;
        }
        case 3:
        {
            current_cmd.type_mask = VELTYPEMASK;
            current_cmd.velocity.x = 0.0;
            current_cmd.velocity.y = 0.0;
            current_cmd.velocity.z = 0.0;
            current_cmd.yaw_rate = 0.0;
            break;
        }
        case 4:
        {
            current_cmd.type_mask = POSTYPEMASK;
            current_cmd.position.x = 0.0;
            current_cmd.position.y = 0.0;
            current_cmd.position.z = 0.0;
            current_cmd.yaw = 0.0;
            break;
        } 
        }
    }

    float CONTROL_CMD_FSM::handleZCmd(float z_input)
    {
        float deadzone = 0.1;
        if(fabs(z_input - 0.5) < deadzone)
        {
            return 0.0;
        }
        else 
        {
            return (z_input - 0.5) * 2.0 * max_vertical_vel;
        }
    }

    float CONTROL_CMD_FSM::handleYawCmd(float r_input)
    {   
        static float target_yaw_rate = 0.0;
        target_yaw_rate = r_input * max_yaw_rate * M_PI / 180;
        return target_yaw_rate;
    }

    void CONTROL_CMD_FSM::returnHomeCallback(const std_msgs::BoolConstPtr &msg)
    {
        start_return.data = msg->data;
    }

    void CONTROL_CMD_FSM::trajCmdCallback(const quadrotor_msgs::PositionCommandConstPtr &msg)
    {
        traj_cmd = *msg;
        have_traj_cmd = true;
    }

    void CONTROL_CMD_FSM::odomCallback(const nav_msgs::OdometryConstPtr &msg)
    {
        odom = *msg;
        odom_pos(0) = odom.pose.pose.position.x;
        odom_pos(1) = odom.pose.pose.position.y;
        odom_pos(2) = odom.pose.pose.position.z;

        odom_vel(0) = odom.twist.twist.linear.x;
        odom_vel(1) = odom.twist.twist.linear.y;
        odom_vel(2) = odom.twist.twist.linear.z;

        odom_orient.w() = odom.pose.pose.orientation.w;
        odom_orient.x() = odom.pose.pose.orientation.x;
        odom_orient.y() = odom.pose.pose.orientation.y;
        odom_orient.z() = odom.pose.pose.orientation.z;
        have_odom = true;
    }

    void CONTROL_CMD_FSM::manualCmdCallback(const mavros_msgs::ManualControlConstPtr &msg)
    {
        user_cmd = *msg;
        have_manual_cmd = true;
    }

    void CONTROL_CMD_FSM::stateCallback(const mavros_msgs::StateConstPtr &msg)
    {
        uav_state = *msg;
        if(msg->armed && !is_armed_prev)
        {
            recordHomePosition();
        }
        is_armed_prev = msg->armed;
    }

    void CONTROL_CMD_FSM::obsStateCallback(const std_msgs::Int16ConstPtr &msg)
    {
        cmd_state = msg->data;
    }

    void CONTROL_CMD_FSM::printfCmdState()
    {
        static string state_str[7] = {"INIT","MANUAL_FLIGHT","WARNING_ALERT","OPERATOR_LIMIT","EMERGENCY_HOVER","RETURN_HOME"};
        cout << "[CMD]: state: " + state_str[cmd_state] << endl;
    }

    void CONTROL_CMD_FSM::cmdTimerCallback(const ros::TimerEvent &e)
    {
        control_cmd_timer.stop();

        current_cmd.header.frame_id = "map";
        current_cmd.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
        //current_cmd.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
        current_cmd.header.stamp = ros::Time::now();

        int previous_state = 0;
        if(cmd_state != previous_state)
        {
            resetCurrentCmd(cmd_state);
            previous_state = cmd_state;
        }
        
        home_pos_pub.publish(home_pos);

        static int fsm_num = 0;
        fsm_num++;
        if(fsm_num == 50)
        {
            if(!have_odom)
            {
                cout<<"no odom!"<<endl;
            }
            if(!have_manual_cmd)
            {
                cout<<"no manual control cmd!"<<endl;
            }
            if(uav_state.mode != "OFFBOARD")
            {
                cout<<"current uav state is not OFFBOARD"<<endl;
            }
            printfCmdState();
            fsm_num = 0;
        }

        if(uav_state.mode != "OFFBOARD")
        {
            //ROS_WARN_STREAM_THROTTLE(1.0,"current uav state is not OFFBOARD");
            
            current_cmd.type_mask = POSTYPEMASK;
            current_cmd.position.x = odom.pose.pose.position.x;
            current_cmd.position.y = odom.pose.pose.position.y;
            current_cmd.position.z = odom.pose.pose.position.z;
            cmd_pub.publish(current_cmd);
        }
        else if(uav_state.mode == "OFFBOARD")
        {
            //获取欧拉角
            tf2::Quaternion q(odom.pose.pose.orientation.w,odom.pose.pose.orientation.x,odom.pose.pose.orientation.y,odom.pose.pose.orientation.z);
            tf2::Matrix3x3 m(q);
            double roll;
            double pitch;
            double yaw;
            m.getRPY(roll,pitch,yaw);

            switch (cmd_state)
            {
            //INIT
            case 0:
            {
                if(!have_odom)
                {
                    goto force_return;
                }
            }
            //MANUAL_FLIGHT
            case 1:
            {
                if(odom_vel.norm() > 0.01) 
                {
                    last_motion_direction = odom_vel.normalized();
                }
            
                //cout<<"current yaw is"<<yaw<<endl;
                // const double kp_vel = 0.8, kd_vel = 0.2;
                // static double last_vel_x = 0, last_vel_y = 0;
                double vx_body = user_cmd.x;
                double vy_body = -user_cmd.y;
                double vx_local = vx_body * cos(yaw) - vy_body * sin(yaw);
                double vy_local = vx_body * sin(yaw) + vy_body * cos(yaw);
                double target_vx = vx_local * max_horizonal_vel;
                double target_vy = vy_local * max_horizonal_vel;
                // double vel_error_x = target_vx - odom_vel.x();
                // double vel_error_y = target_vy - odom_vel.y();
                // double pd_vel_x = kp_vel * vel_error_x + kd_vel * (vel_error_x - last_vel_x) / 0.02;
                // double pd_vel_y = kp_vel * vel_error_y + kd_vel * (vel_error_y - last_vel_y) / 0.02;

                current_cmd.type_mask = VELTYPEMASK;
                current_cmd.velocity.x = target_vx;
                current_cmd.velocity.y = target_vy;
                // current_cmd.velocity.x = user_cmd.x;
                // current_cmd.velocity.y = -1 * (user_cmd.y);
                
                //current_cmd.velocity.z = user_cmd.z;

                current_cmd.velocity.z = handleZCmd(user_cmd.z);
                current_cmd.yaw_rate = handleYawCmd(user_cmd.r);
                break;
            }

            //WARNING_ALERT
            case 2:
            {
                //ROS_WARN_STREAM_THROTTLE(1.0,"Obstacle detected!");
                if(odom_vel.norm() > 0.01) 
                {
                    last_motion_direction = odom_vel.normalized();
                }
                // const double kp_vel = 0.8, kd_vel = 0.2;
                // static double last_vel_x = 0, last_vel_y = 0;
                double vx_body = user_cmd.x;
                double vy_body = -user_cmd.y;
                double vx_local = vx_body * cos(yaw) - vy_body * sin(yaw);
                double vy_local = vx_body * sin(yaw) + vy_body * cos(yaw);
                double target_vx = vx_local * max_horizonal_vel;
                double target_vy = vy_local * max_horizonal_vel;
                // double vel_error_x = target_vx - odom_vel.x();
                // double vel_error_y = target_vy - odom_vel.y();
                // double pd_vel_x = kp_vel * vel_error_x + kd_vel * (vel_error_x - last_vel_x) / 0.02;
                // double pd_vel_y = kp_vel * vel_error_y + kd_vel * (vel_error_y - last_vel_y) / 0.02;

                current_cmd.type_mask = VELTYPEMASK;
                current_cmd.velocity.x = target_vx;
                current_cmd.velocity.y = target_vy;
                // current_cmd.velocity.x = user_cmd.x;
                // current_cmd.velocity.y = -1 * (user_cmd.y);
                
                //current_cmd.velocity.z = user_cmd.z;
                current_cmd.velocity.z = handleZCmd(user_cmd.z);
                current_cmd.yaw_rate = handleYawCmd(user_cmd.r);
                break;
            }

            //OPERATOR_LIMIT
            case 3:
            {
                //ROS_WARN_STREAM_THROTTLE(1.0,"operator limit!");

                double vx_body = user_cmd.x;
                double vy_body = -user_cmd.y;
                double vx_local = vx_body * cos(yaw) - vy_body * sin(yaw);
                double vy_local = vx_body * sin(yaw) + vy_body * cos(yaw);

                Eigen::Vector3d motion_direction = odom_vel.normalized();
  
                //将输入的速度投影到受限方向与平行方向
                Eigen::Vector3d user_vel(vx_local * max_horizonal_vel,vy_local * max_horizonal_vel,0.0);
                //对速度进行平滑处理
                static Eigen::Vector3d prev_smoothed_vel = Eigen::Vector3d::Zero();
                double alpha = 0.5 + 0.4 * (1.0 - odom_vel.norm() / max_horizonal_vel);
                Eigen::Vector3d smoothed_vel = alpha * user_vel + (1 - alpha) * prev_smoothed_vel;
                prev_smoothed_vel = smoothed_vel;


                double vel_along_restricted = smoothed_vel.dot(motion_direction);
                //对受限方向的速度进行限制
                double limit_vel = vel_along_restricted * vel_limit_factor;
                Eigen::Vector3d final_vel = limit_vel * motion_direction + (user_vel - vel_along_restricted * motion_direction);


                current_cmd.type_mask = VELTYPEMASK;
                current_cmd.velocity.x = final_vel.x();
                current_cmd.velocity.y = final_vel.y();

                current_cmd.velocity.z = handleZCmd(user_cmd.z);
                current_cmd.yaw_rate = handleYawCmd(user_cmd.r);
    
                break;
            }

            //EMERGENCY_HOVER
            // case 4:
            // {

            //     static ros::Time emergency_start_time = ros::Time::now();
            //     static ros::Time back_up_start_time;
            //     const double max_backup_vel = 0.5;
            //     const double backup_dis = 1.0;

            //     if(odom_vel.norm() > 0.01)
            //     {
            //         last_motion_direction = odom_vel.normalized();
            //         is_backing_up = false;
            //     }
            
            //     if((ros::Time::now() - emergency_start_time).toSec() < emergency_hover_duration)    
            //     {
            //         ROS_WARN("emergency hover!");
            //         current_cmd.type_mask = POSTYPEMASK; 
            //         current_cmd.position.x = odom.pose.pose.position.x;
            //         current_cmd.position.y = odom.pose.pose.position.y;
            //         current_cmd.position.z = odom.pose.pose.position.z;
            //     }
            //     else
            //     {
            //         if(!is_backing_up && last_motion_direction.norm() < 0.01)
            //         {
            //             Eigen::Vector3d forward_direction = odom_orient * Eigen::Vector3d::UnitX();
            //             last_motion_direction = -forward_direction;
            //         }
            //         is_backing_up = true;
            //         Eigen::Vector3d backup_pos = -last_motion_direction * backup_dis; 
            //         current_cmd.type_mask = POSTYPEMASK; 
            //         current_cmd.position.x = backup_pos.x();
            //         current_cmd.position.y = backup_pos.y();
            //         current_cmd.position.z = 0;
            //         current_cmd.yaw = 0;
            //     }
            //     break;
            // }
            case 4:
            {
                //ROS_WARN_STREAM_THROTTLE(1.0,"emergency hover!");
                current_cmd.type_mask = POSTYPEMASK; 
                current_cmd.position.x = odom.pose.pose.position.x;
                current_cmd.position.y = odom.pose.pose.position.y;
                current_cmd.position.z = odom.pose.pose.position.z;

                break;
            }

            //RETURN_HOME
            case 5:
            {
                static double current_x = odom_pos(0);
                static double current_y = odom_pos(1);
                static double current_z = odom_pos(2);
                //ROS_WARN_STREAM_THROTTLE(1.0,"return home!");
                if(!have_traj_cmd ||!start_return.data)
                {
                    current_cmd.type_mask = VELYAWTYPEMASK;
                    current_cmd.velocity.x = (current_x - odom_pos(0)) * 1.0;
                    current_cmd.velocity.y = (current_y - odom_pos(1)) * 1.0;
                    current_cmd.velocity.z = (current_z - odom_pos(2)) * 1.0;
                    current_cmd.yaw = yaw;
                    current_x = odom_pos(0);
                    current_y = odom_pos(1);
                    current_z = odom_pos(2);
                    //ROS_WARN("no return trajectory cmd!");

                }
                else
                {
                    current_cmd.type_mask = VELYAWTYPEMASK;
                    current_cmd.velocity.x = 0.5 * traj_cmd.velocity.x + (traj_cmd.position.x - odom_pos(0)) * 1.0;
                    current_cmd.velocity.y = 0.5 * traj_cmd.velocity.y + (traj_cmd.position.y - odom_pos(1)) * 1.0;
                    current_cmd.velocity.z = (traj_cmd.position.z - odom_pos(2)) * 1.0;
                    current_cmd.yaw = traj_cmd.yaw;
                }
                break;
            }
            }


            cmd_pub.publish(current_cmd);
        }

        force_return:;
            control_cmd_timer.start();
    }
}//namespace obstacle_avoid
